#include "OI.h"
#include "Commands/PopUpClimber.h"
#include "Commands/PullUpClimber.h"
#include "Commands/DiscPickupControl.h"
#include "Commands/AutoShoot.h"
#include "Commands/FireDisc.h"
#include "Commands/BreachControl.h"
#include "Commands/ShooterAngleControl.h"
#include "Commands/ConnectToDash.h"
#include "Commands/ChangeShooterAngle.h"
#include "Commands/SetShooterAngle.h"
#include "Commands/ChangeSweeperStatus.h"
#include "Commands/PulseBreach.h"
#include "Commands/BreachDisc.h"

OI::OI() :
	driverJoystick(1), shooterJoystick(2) {
	// Process operator interface input here.
	//DRIVER CONTROLS
//	JoystickButton *driverButton1 = new JoystickButton(&driverJoystick, 1);
//	JoystickButton *driverButton2 = new JoystickButton(&driverJoystick, 2);
	//	JoystickButton *driverButton3 = new JoystickButton(&driverJoystick, 3);
	JoystickButton *driverButton4 = new JoystickButton(&driverJoystick, 4);
	JoystickButton *driverButton5 = new JoystickButton(&driverJoystick, 5);
	JoystickButton *driverButton6 = new JoystickButton(&driverJoystick, 6);
	//	JoystickButton *driverButton8 = new JoystickButton(&driverJoystick, 8);
//	driverButton1->WhenPressed(new AutoShoot());
//	driverButton4->WhenPressed(new ConnectToDash());
//	driverButton4->WhenPressed(new ChangeSweeperStatus());
	driverButton5->WhenPressed(new PopUpClimber());
	driverButton6->WhenReleased(new PullUpClimber());
	//SHOOTER CONTROLS
		JoystickButton *shooterButton1 = new JoystickButton(&shooterJoystick, 1);
		JoystickButton *shooterButton2 = new JoystickButton(&shooterJoystick, 2);
		JoystickButton *shooterButton3 = new JoystickButton(&shooterJoystick, 3);
		JoystickButton *shooterButton4 = new JoystickButton(&shooterJoystick, 4);
		JoystickButton *shooterButton5 = new JoystickButton(&shooterJoystick, 5);
		JoystickButton *shooterButton6 = new JoystickButton(&shooterJoystick, 6);
		JoystickButton *shooterButton7 = new JoystickButton(&shooterJoystick, 7);
		JoystickButton *shooterButton10 = new JoystickButton(&shooterJoystick, 10);
		JoystickButton *shooterButton11 = new JoystickButton(&shooterJoystick,11);
		JoystickButton *shooterButton8 = new JoystickButton(&shooterJoystick,8);
		JoystickButton *shooterButton9 = new JoystickButton(&shooterJoystick,9);
		shooterButton1->WhenPressed(new FireDisc());
		shooterButton7->WhenPressed(new ChangeShooterAngle(-1.5));
		shooterButton6->WhenPressed(new ChangeShooterAngle(1.5));
		shooterButton3->WhenPressed(new SetShooterAngle(40.0));
		shooterButton4->WhenPressed(new PulseBreach());
		shooterButton8->WhenPressed(new ChangeSweeperStatus());
		shooterButton2->WhenPressed(new SetShooterAngle(3));
		shooterButton10->WhenPressed(new ChangeSweeperStatus());
}
float OI::GetXValue() {
	return driverJoystick.GetX();
}
float OI::GetYValue() {
	return driverJoystick.GetY();
}
float OI::GetThrottleValue() {
	return driverJoystick.GetZ();
}
bool OI::GetIfFeeder() {
	//		return shooterJoystick.GetRawButton(3);
	return false;
//	printf("Feeding...\n");
}

void OI::CheckButtons() {
	for (int i = 1; i <= 12; i++) {
		shooterDown[i] = shooterJoystick.GetRawButton(i);
		driverDown[i] = driverJoystick.GetRawButton(i);
	}
}
bool OI::GetShooterOverride(){
	return CommandBase::oi->shooterJoystick.GetRawButton(2);
}
